package astar;

import java.util.ArrayList;
import java.util.Comparator;
import java.util.Iterator;
import java.util.List;
import java.util.PriorityQueue;

public class AStar {
    private Node[][] map;
    private PriorityQueue<Node> openList;// 11

    public static Comparator<Node> F_Comparator = new Comparator<Node>() {
        @Override
        public int compare(Node c1, Node c2) {
            return (int) (c1.getF() - c2.getF());
        }
    };

    public AStar(int[][] map) {
        this.map = new Node[map.length][map.length];
        for (int x = 0; x < map.length; x++) {
            for (int y = 0; y < map.length; y ++) {
                this.map[x][y] = new Node(x, y, map[y][x]);
            }
        }
        this.openList = new PriorityQueue<>(10, F_Comparator);
    }

    public List<Node> FindPath(int x1, int y1, int x2, int y2) {
        Node startNode = this.map[x1][y1];
        Node endNode   = this.map[x2][y2];
        List<Node> result = new ArrayList<Node>();
        boolean IsFind = false;

        this.openList.add(startNode);
        this.map[startNode.getX()][startNode.getY()].setStatus(11);// Status 11 means in the openlist
        Node currentNode = this.openList.poll();
        while (currentNode != null) {
            for (int i= 0;i<4;i++) {
                switch (i) {
                case 0:// right
                    if ( (currentNode.getX() + 1) < map.length) 
                    	CheckNode(this.map[currentNode.getX() + 1][currentNode.getY()], endNode, currentNode,1);
                    break;
                case 1:// down
                    if ( (currentNode.getY() + 1) < map.length) 
                    	CheckNode(this.map[currentNode.getX()][currentNode.getY() + 1], endNode, currentNode, 1);
                    break;
                case 2:// left
                    if ( (currentNode.getX() - 1) >= 0) 
                    	CheckNode(this.map[currentNode.getX() - 1][currentNode.getY()], endNode, currentNode, 1);
                    break;
                case 3:// up
                    if ( (currentNode.getY() - 1) >= 0) 
                    	CheckNode(this.map[currentNode.getX()][currentNode.getY() - 1], endNode, currentNode, 1);
                    break;
                } 
            }
            IsFind = this.AddToClosedList(currentNode, endNode);
            if (IsFind) {
                break;
            }
            currentNode = this.openList.poll();
//            UpdateList(currentNode);
        }
        if (IsFind) {
            ReadList(result, currentNode);
            return result;
        } else {
            return result;
        }
    }

    private void ReadList(List<Node> resultList, Node node) {
        if (node.getParent() != null) {
            ReadList(resultList, node.getParent());
        }
        resultList.add(0, node);
    }
    
    private boolean DetectNeighbour(Node node, Node parent) {
        boolean flag = false;
        if (node.getX() != parent.getX() && node.getY() != parent.getY()) {
            if (this.map[node.getX()][parent.getY()].getBlock() == 1) {
                flag = true;
            }
            if (this.map[parent.getX()][node.getY()].getBlock() == 1) {
                flag = true;
            }
        }
        return flag;
    }

    private boolean CheckNode(Node node, Node endNode, Node parent, int step) {
        if (node.getBlock() == 1) {
            //blocked
            return false;
        }
        if (node.getStatus() == 22) {
            //on closeList
            return false;
        }
        if (node.getStatus() == 0) {
            //not visited
            if (DetectNeighbour(node, parent)) {
                return false;
            }
            this.AddToOpenList(node, endNode, parent, step);
            return true;
        } 
        else if (node.getStatus() == 11) {
            this.UpdateOpenList(node, endNode, parent, step);
            return true;
        }
        return false;
    }

    private boolean AddToClosedList(Node node, Node endNode) {
        if (node.getX() == endNode.getX() && node.getY() == endNode.getY()) {
            return true;
        }
        this.map[node.getX()][node.getY()].setStatus(22);
        return false;
    }

    private boolean AddToOpenList(Node node, Node endNode, Node parent, int step) {
        node.setParent(parent);
        this.CountValue(node, endNode, step);
        this.openList.add(node);
        node.setStatus(11);
        return true;
    }

    private boolean UpdateOpenList(Node node, Node endNode, Node parent, int step) {
        for (Node nd : this.openList) {
            if (node.equals(nd)) {
                node = nd;
                break ;
            }
        }
        int g = parent.getG() + step;
        if (g < node.getG()) {
            node.setParent(parent);
            this.CountValue(node, endNode, step);
            this.openList.remove(node);
            this.openList.add(node);
            return true;
        }
        return false;
    }

    private void CountValue(Node node, Node endNode, int step) {
        this.CountG(node, node.getParent(), step);
        this.CountH(node, endNode);
        this.CountF(node);
    }

    private void CountG(Node node, Node parent, int step) {
        if (parent == null) {
            node.setG(step);
        } else {
        	node.setG(parent.getG() + step);
        }
    }

    private void CountH(Node node, Node endNode) {
        node.setH((Math.abs(endNode.getX() - node.getX()) + Math.abs(endNode.getY() - node.getY())));
    }

    private void CountF(Node node) {
        node.setF(node.getG() + node.getH());
    }
    
    private Node UpdateH(Node node,Node pre){
        pre.setH(Math.max(pre.getH(),node.getG() - pre.getG()));
        return pre;
    }
    
    private void UpdateList(Node node){
        List<Node> list = new ArrayList<Node>();
        while(!openList.isEmpty()){
            list.add(UpdateH(node, openList.poll()));
        }
        for (int i=0;i<list.size();i++){
            openList.add(list.get(i));
        }
    }
    
}